//Erik Wright
//bot_motion.cpp

/*
This .cpp file describes the functions listed in bot_motion.h.
*/


#include "bot_motion.h"
#include <iostream>		//standard input output
#include <cstdlib>		//rand, srand
#include <ctime>			//time
#include <cmath>			//ceil, floor
using namespace std;


//----------------------------motion_event-----------------------------
//====================private functions

/* display_event
This function merely displays the motion data encapsulated in the class instance.  It is meant to be called from a recursive algorithm that traverses the entire list.
INPUT:	none
OUTPUT:	standard display output
*/
void motion_event::display_event()
{
	cout<<"P"<<pulse_width<<"  ";
	cout<<"T"<<rot_time<<"  ";
	cout<<"S"<<ramp_speed<<"  ";
	cout<<"\t";
}



/* recursive_delete is a private function that is paired with the deconstructor.
It will delete every item after a given item before the first instance of the 
function returns.  This would be a very useful function to use when preparing the 
crossover of strings in an IGA.
INPUT:	pointer to a motion_event
OUTPUT:	none
*/
void motion_event::recursive_delete(motion_event * given)
{
	if(given)	//next pointer has some value
	{
		given->recursive_delete(given->next);	//recurse to end of list
		delete given;			//remove the end of the list
		given = NULL;
		return;
	}
	else		//next pointer is null
	{
		return;
	}
}




/*
insert_at_end_recur()
This function recursively searches down the list to find the end and
inserts the given element or list pointed to by to_ins, and inserts them
there.  This program will change to_ins to a void pointer.  This makes 
sure that multiple pointers don't point to the same place at the same 
time.  Be ready to have this pointer changed after you give it to the
function.
INPUT:	motion_event pointer: for recursion
	motion_evetn by refference: given list to insert
OTUPUT:	none
*/
void motion_event::insert_at_end_recur(motion_event* given_node, motion_event *& to_ins)
{
	if(given_node)		//not the end of list
	{	
		if(given_node->next)		//there is data behind given pointer
		{				//then go to the end pointer in list
			given_node->insert_at_end_recur(given_node->next, to_ins);
			return;
		}	
		else			//data should go behind current pointer
		{
			given_node->next = to_ins;
			to_ins = NULL;
			return;
		}
	}
}



/*
recursive_inorder_display()
this function is a recursive function that travels the legnth of the 
linear linked list displaying all nodes in the list past the calling
node.  This function is intended for debugging purposes and realtime
command display.
INPUT:	motion_event pointer
OUTPUT:	recursive display 
*/
void motion_event::recursive_inorder_display(motion_event* given)
{
	if(given)	//not end of list
	{
		given->display_event();	//display event
		recursive_inorder_display(given->next);	//recurse to next node
		return;
	}
	else		//end of list
	{
		cout<<"|";
		return;
	}
}




/*
recur_deep_cpy()
This function performs a recursive deep copy of all data 
in the to_cpy pointer.  It creates a new instance with those
values in the next field of the current_node variable.
INPUT:	current_node | pointer to the current node in the construction 
	to_cpy	| pointer to the data to be copied
OUTPUT:	complete deep copy of class data
*/
void motion_event::recur_deep_cpy(motion_event*& current_node, motion_event* to_cpy)
{
	if(to_cpy)    //this is not last node
	{
        	current_node = new motion_event(to_cpy->pulse_width, to_cpy->ramp_speed, to_cpy->rot_time);
    		current_node->recur_deep_cpy(current_node->next, to_cpy->next);
		return;
	}
	else		//this is the last node
	{
		return;
	}	
}



/*
lengthof(motion_event*)
This function recursively finds the length of the array behind a given 
pointer.  The return value is the length of the list.
INPUT:	motion_event pointer | place to begin length.
OUTPUT:	integer | length of list
*/
int motion_event::lengthof(motion_event* start)
{
	if(start)	//not end of list
	{
		return 1 + start->lengthof(start->next);
	}
	else
	{
		return 0;
	}
}
		


/*
goto_event(int)
this function will traverse the linked list behind the given pointer
until it reaches the specified depth.  it will then return a pointer 
to that element in the list.
INPUT:	integer | traversal depth
	motion_event pointer | list to traverse
OUTPUT:	motion_event pointer | instance at specified depth
*/
motion_event* motion_event::goto_event(motion_event* given, int depth)
{
	if(given)	//not end of list
	{
		if(depth)	//haven't reach depth
		{
			return given->goto_event(given->next, --depth);
		}
		else		//have reaced specified depth
		{
			return given;
		}
	}
	else		//end of list
	{
		if(depth)	//error: haven't reached depth and out of nodes
		{
			cout<<"ERROR: Specified depth of traversal has overrun length of list."<<endl;
			return NULL;
		}
		else		//the end is the swap point
		{
			return NULL;
		}
	}
}



/*
remove_from_swappoint(motion_event*&, int)
This function will split a list of events at the spot designated by the given 
integer.  It will return this list with a pointer to the beginning node in the list.
This is meant to be paired with the insert_at_end() function while performing
a chomosome twist.
INPUT:	motion_event*& | motion_event to begin the search
	integer | depth to traverse down list
OTUPUT:	motion-event* | pointer to the separated list of events
*/
motion_event* motion_event::remove_from_swappoint(motion_event*& given, int depth)
{
	if(given)	//there is data left to traverse through
	{
		if(depth)	//we have not yet reached the destined depth
		{
			return given->remove_from_swappoint(given->next, --depth);
		}
		else		//we have reached specified depth
		{
			motion_event* temp = given;	//store value of given ptr
			given = NULL;		//nullify end of list, mark the end
			return temp;		//return stored value of given ptr
		}
	}
	else		//there is no more data left in list
	{
		if(depth)	//we have not reached specified depth
		{
			cout<<"ERROR: specified depth out of range for list"<<endl;
			return NULL;
		}
		else	//the end is the swap point
		{
			return given;
		}
	}	

}



//recur_avg()
/*
This function produces requires that the argument "build" be a null pointer.
This function will fill that null pointer with a string of averages.

INPUT:	motion_event*&	| list to average
	motion_event*&	| list to average with
	motion_event*& 	| newly constructed list of averages
OUTPUT: void
*/
void motion_event::recur_avg(motion_event*& given_a, motion_event*& given_b, motion_event*& build)
{
	if((given_a != NULL)&&(given_b != NULL))	//continue recuring
	{
		int pw_avg = static_cast<int>(floor((given_a->pulse_width + given_b->pulse_width)/2));
		int t_avg = static_cast<int>(floor((given_a->rot_time + given_b->rot_time)/2));
		int s_avg = static_cast<int>(floor((given_a->ramp_speed + given_b->ramp_speed)/2));
		build = new motion_event(pw_avg, t_avg, s_avg);
		recur_avg(given_a->next, given_b->next, build->next);
		return;
	}
	else
	{
		return;
	}
}	
		
			
//======================public functions




/*
~motion_event() is the class deconstructor, it recursively deletes all instances
of the motion_event class in a list of motion_event classes prior to the class that 
it was called on, and then the instance that it was called on. 
INPUT:	void
OUTPUT:	class deconstruction
*/
motion_event::~motion_event()
{
	if(this->next)	//there is more than one element in list
	{
	this->recursive_delete(this->next);
	delete this->next;
	}
	return;
}




/*motion_event()
this function acts as the class constructor.  It initializes a random value
for the angle of rotation for the specific servo.
INPUT: none
OUTPUT:	instance initialization
*/
motion_event::motion_event()
{

	pulse_width = rand() % 2000;	//randomly choose a pulse width 
	pulse_width += 500;	//set min/max offset

	rot_time = rand() % 65535; 	//randomly choose a rotation time
	
	ramp_speed = rand() % 200000;	//randomly choose ramp speed uSec/Sec  THIS COULD BE INTERESTING TO PLAY WITH
	next = NULL;	//nullify new link
}




/*
motion_event(int, int, int)
This function acts as a loaded class constructor.
When it is invoked, the number of degrees to turn the motor
is given as an argument.
INPUT:	integer rota_val | the fixed value for the rotation of the motor
OUTPUT:	initialization
*/
motion_event::motion_event(int pw_val, int rmp_spd, int event_time):
	pulse_width(pw_val),
	ramp_speed(rmp_spd),
	rot_time(event_time),
	next(NULL)
{
}




/*
motion_event(motion_event &)
This is the class copy constructor.  Given a built instance of the 
class, it creates a copy of the class.  It will perform a deep copy
of any initialized data behind the class instance being copied.  
Be aware that this might be a very large linear linked list.
INPUT:	motion_event by refference
OUTPUT:	class initialization
*/
motion_event::motion_event(const motion_event& given):
	pulse_width(given.pulse_width),
	ramp_speed(given.ramp_speed),
	rot_time(given.rot_time)
{
	if(this->next)	//our list contains elements
	{
		delete this->next;
		recur_deep_cpy(this->next, given.next);	//copy entire list to this point
		return;
	}
	else		//our list contains only one instance
	{
		return;
	}
}




/*
insert_at_end()
This function inserts a node or entire list of nodes to the end of an existing list.
INPUT: motion_event by refference: for insertion, this pointer will be nullified
OUTPUT:	none	
*/
void motion_event::insert_at_end(motion_event *& to_ins)
{
	if(to_ins)	//we are being given something to insert
	{
		if(this)	//there exists a list of any size
		{
			if(this->next)	//there is more than one element in list
			{		//begin recursive function to end
				this->insert_at_end_recur(this->next, to_ins);
				return;
			}
			else		//we should just insert the data here
			{		//since it is the first element
				this->next = to_ins;
				to_ins = NULL;
				return;
			}
		}
		else
		{
			cout<<"WARNING: Null this*"<<endl;
			return;
		}
	}	
	else
	{
		cout<<"Warning: Trying to insert data behind a null pointer"<<endl;
		return;
	}
}





/*
inorder_display()
This is a recursive function wrapper that displays the list 
behind a given node.
INPUT:	motion_event pointer
OUTPUT:	to standard out
*/
void motion_event::inorder_display()
{

	display_event();		//display the current event

	if(this->next)	//there is more than one element to display
	{
		recursive_inorder_display(next);
	}

	cout<<endl;
}



/* binary_twist(motion_event*&)
This function performs a binary twist at the midpoint between two chromosomes.
It begins by finding the length of each list and then splitting them both at 
thier midpoint.  The two segments are then swapped places, thus performing a 
simple binary twist on two chromosomes.  Swapping chromosomes of different
length will produce different final lengths for both chromosomes.
INPUT:	motion_event by refference| list to twist with
OUTPUT:	a binary genetic twist
*/
void motion_event::binary_twist(motion_event*& given, bool round)
{
	//find length of both lists
	int this_len = this->lengthof(this);
	int that_len = given->lengthof(given);
	int swappoint_a, swappoint_b = 0;	//variables and initial value


	if(this_len == that_len)	//equal lengths, a good general case
	{
		if(this_len%2)	//these lists are of odd length
		{
			//the rounding should be determined by rating
			if(round)	//this should be rounded up
			{
				swappoint_a = swappoint_b = static_cast<int> (.5 + ceil(this_len/2));
			}		
			else		//round down
			{
				swappoint_a = swappoint_b = static_cast<int> (floor(this_len/2));
			}
		}
		else		//these lists are of even length
		{
			swappoint_a = swappoint_b = this_len/2;
		}
	}
	else			//lists are of different length
	{
		if(round)	//either round up or down
		{
			//round swap points up
			swappoint_a = static_cast<int> (.5 + ceil(this_len/2));
			swappoint_b = static_cast<int> (.5 + ceil(that_len/2));
		}
		else
		{
			//round swap points down 
			swappoint_a = static_cast<int> (floor(this_len/2));
			swappoint_b = static_cast<int> (floor(that_len/2));
		}
	}


		//execute the swap
	motion_event* placeholder_a = remove_from_swappoint(given, swappoint_b);
	motion_event* placeholder_b = remove_from_swappoint(this->next, swappoint_a-1);
	this->insert_at_end(placeholder_a);
	given->insert_at_end(placeholder_b);			
}





/* binary_twist_rand(motion_event*&)
This function performs a binary twist at a random point in two chromosomes.
It begins by finding the length of each list and using that to mod a random call
ensuring that they will be split within the length of the chomosome.
The two segments are then swapped places, thus performing a 
simple twist on two chromosomes.  This is designed to be used with chromosomes
of varying length.
If the keep_length flag is set, the function will keep the length of equally
lengthed sets.  If the two chromosomes have different length, this flag is
ignored.
INPUT:	motion_event by refference| list to twist with
OUTPUT:	a binary genetic twist
*/
void motion_event::binary_twist_rand(motion_event*& given, bool keep_length)
{
	//get lengths
	int len_a = this->lengthof(this);
	int len_b = given->lengthof(given);
	int swp_pt_a, swp_pt_b = 0;

	if((keep_length) && (len_a == len_b))
	{
		swp_pt_a = swp_pt_b = rand() % len_a + 1;	//random swap point in
		
		if(swp_pt_a == len_a)	//we might not even be swapping anything
		{
			//swap in center
			swp_pt_a = swp_pt_b = static_cast<int>(.5 + ceil(swp_pt_a / 2));
		}
	} 
	else
	{	 
		swp_pt_a = rand() % len_a + 1;	//don't care about maintaining length
		swp_pt_b = rand() % len_b + 1;

		if(swp_pt_a == len_a)	//we do care about keeping the final command the same 
		{
			swp_pt_a = static_cast<int>(.5 + ceil(swp_pt_a / 2));
		}
		if(swp_pt_b == len_b)
		{
			swp_pt_b = static_cast<int>(.5 + ceil(swp_pt_b / 2));
		}
	}

	//execute swap
	motion_event* placeholder_a = remove_from_swappoint(given, swp_pt_b);
	motion_event* temp = this;
	motion_event* placeholder_b = remove_from_swappoint(temp, swp_pt_a);
	this->insert_at_end(placeholder_a);
	given->insert_at_end(placeholder_b);
}




/*
average_twist(motion_event*&)
this function performs a genetic twist based on averaging the two chromosomes
together.  this function can be used as a pure average of two chromosomes.
Bear in mind that when twisting chromosomes of different lengths, the elements 
that are missing corrospond to the "all-stop" state.
INPUT:	motion_event by refference| list to twist with
OTUPUT:	an average genetic twist
*/
void motion_event::average_twist(motion_event*& given)
{
	//find length of both lists
	int this_len = this->lengthof(this);
	int that_len = given->lengthof(given);

	//are the lengths equal in length?
	if(this_len == that_len)	//a good general case
	{
		if(given != NULL)
		{
			//create average list
			motion_event* temp = this;
			motion_event* average = NULL;
			recur_avg(temp , given, average);

			//swap average list with others
			binary_twist_rand(average, 1);		//keep length
			given->binary_twist_rand(average,1);	//keep length	
			return;
		}
		else
		{
			return;
		}
	}
	else			//lists are of different length
	{
		cout<<"WARNING: averaging two lists of different size"<<endl;
		return;	
	}


}



/*
weighted_average_twist(motion_event*&)
This function performs a simple weighted twist between two chromosomes of 
different or equal length.  The weight is dependant on the rating of the
the chromosome.  Highly rated chromosomes will be of higher weight and 
will result in higher changes
INPUT:	motion_event*&| list to twist with
	integer | weight of the average
OTUPUT:	a weighted average twist
*/
void motion_event::weighted_average_twist(motion_event*& given, int weight)
{
	//find length of both lists
	int this_len = this->lengthof(this);
	int that_len = given->lengthof(given);

	//are the lengths equal in length?
	if(this_len == that_len)	//a good general case
	{
		if(this_len%2)	//this list is of odd length
		{
			if(that_len%2)	//that list is of odd length
			{
	
			}
			else		//that list is of even length
			{
		
			}
		}
		else		//this list is of even length
		{
			if(that_len%2)	//that list is of odd length
			{
	
			}
			else		//that list is of even length
			{
		
			}
		}
	}
	else			//lists are of different length
	{
		if(this_len%2)	//this list is of odd length
		{
			if(that_len%2)	//that list is of odd length
			{
	
			}
			else		//that list is of even length
			{
		
			}
		}
		else		//this list is of even length
		{
			if(that_len%2)	//that list is of odd length
			{
	
			}
			else		//that list is of even length
			{
		
			}
		}
	}
}
